
#include <string>

#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/time.h>
#include <signal.h>

#include "nmc.h"
nmc_port *port;

using std::string;

void handler(int signal) {
  printf("Shuting down...\n");
  if (port)
    nmc_shutdown(port, 0x80);
  exit(0);
}

// Do it all
int main (int argc, char **argv) 
{
  struct timeval start, now;
  int nummod;
  int ix, x;
  string dev = "/dev/ttyS5";
  if (argc > 1)
    dev = argv[1];

  signal(SIGINT, handler);

  printf("Initializing...\n");
  //nummod = nmc_init(230400, dev, &port);
  nummod = nmc_init(115200, dev.c_str(), &port);
  //nummod = nmc_init(57600, dev, &port);
  //nummod = nmc_init(19200, dev, &port);
  printf("Found %d modules\n", nummod);
  
  nmc_set_group(port, 1, 0x80, 1);
  for (ix=2; ix<=nummod;ix++) {
    nmc_set_group(port, ix, 0x80, 0);
  }


  //  for (int j=0;j<10;j++) {
  gettimeofday(&start, NULL);
  //for (int i=0;i<10;i++) {
  for (;;) {
    for(ix=1; ix<=nummod; ix++) {
      printf("Reading position...\n");
      printf("Position: %ld\n", nmc_get_position(port, ix, true));
      //nmc_get_position(port, ix, true);
      //usleep(15*1000);
      //sleep(1);
      
      /*
	printf("Setting the gain ##%d...\n", ix);
	nmc_set_pid_parameters(port, ix,30,0,100);
	sleep(1);
	
      printf("Turning the servo on...\n");
      nmc_motors_on(port, ix);
      sleep(1);
      
      printf("Moving...\n");
      nmc_set_position(port, ix, 1000, (long)(3*30557.75));
      nmc_wait_position(port, ix);
      sleep(5);
      
      nmc_set_position(port, ix, 0, (long)(3*30557.75));
      nmc_wait_position(port, ix);
      //sleep(1);
      */
    }
  }
  gettimeofday(&now, NULL);
  double elapsed = now.tv_sec-start.tv_sec+
    ((float)(now.tv_usec-start.tv_usec))/1000000;
  printf("Time: %g  Average: %g  Hz: %g\n", 
	 elapsed, (elapsed/10), 1/(elapsed/10));
  
  printf("Shutting down...\n");
  if (nummod)
    nmc_shutdown(port, 0x80);
  
  return 0;
}
